/*
 * myaffine.c
 *
 *  Created on: Jul 22, 2012
 *      Author: Nautilus
 */

#include "myaffine.h"
#include <stdlib.h>
#include <math.h>

void initMatrix(MATRIX *a)
{
	if(a != 0)
	{
		a->m = 0;
		a->nRow = 0;
		a->nCol = 0;
	}
}

void releaseMatrix(MATRIX *m)
{
	if(m != 0)
	{
		if(m->m != 0)
			free(m->m);
	}
	/* Please don't release m itself */
}

/*
 *  Calculate R = A * B
 * */
unsigned char multiply(double *A[], int rowA, int colA, double *B[], int rowB, int colB, double *R[])
{
	double s;
	int i, j, k;
	if(colA != rowB)
		return 0;

	for(i=0; i < rowA; i++)
	{
		for(j=0; j < colB; j++)
		{
			s = 0;
			for(k=0; k < colA; k++)
				s += A[i][k] * B[k][j];
			R[i][j] = s;
		}
	}
	return 1;
}

unsigned char multiplyM(MATRIX A, MATRIX B, MATRIX *m)
{
	double s;
	int i, j, k;
	if(A.nCol != B.nRow)
		return 0;
	m->m = (double *)malloc(sizeof(double)*(A.nCol * B.nCol));
	for(i=0; i<A.nRow; i++)
	{
		for(j=0; j<B.nCol; j++)
		{
			s = 0;
			for(k=0; k<A.nCol; k++)
				s += A.m[i*A.nCol+k] * B.m[k*B.nCol + j];
			m->m[i*m->nCol + j] = s;
		}
	}
	return 1;
}

/*
 * 	This calculates transpose matrix of A
 * 	INPUT:
 * 		A[rowA][colA]: matrix that will be transpose
 * 		T: will the transpose matrix of A.
 * 		T must be initialized and has size [colA][rowA]
 */
unsigned char T(double *A[], int rowA, int colA, double *T[])
{
	int i, j;
	for(i=0; i<rowA; i++)
		for(j=0; j<colA; j++)
			T[j][i] = A[i][j];
	return 1;
}

/* make affine transformation */
unsigned char affineTransform(double m[4][4], double *p, double *R)
{
	int i, j;
	double s;

	for(i=0; i<4; i++)
	{
		s = 0;
		for(j=0; j<4; j++)
			s = s + m[i][j] * p[j];
		R[i] = s;
	}

	return 1;
}

unsigned char affineTransformM(MATRIX m, double P[], double R[])
{
	int i, j;
	double s;

	if(m.nCol != 4)
		return 0;

	for(i=0; i<4; i++)
	{
		s = 0;
		for(j=0; j<4; j++)
			s = s + m.m[i * m.nCol + j] * P[j];
		R[i] = s;
	}

	return 1;
}

void translate(double tx, double ty, double tz, double *m[])
{
	/*
		{1,  0,  0,  0},
		{0,  1,  0,  0},
		{0,  0,  1,  0},
		{tx, ty, tz, 1}};
	*/
	m[0][0] = 1;
	m[0][1] = 0;
	m[0][2] = 0;
	m[0][3] = 0;

	m[1][0] = 0;
	m[1][1] = 1;
	m[1][2] = 0;
	m[1][3] = 0;

	m[2][0] = 0;
	m[2][1] = 0;
	m[2][2] = 1;
	m[2][3] = 0;

	m[3][0] = tx;
	m[3][1] = ty;
	m[3][2] = tz;
	m[3][3] = 1;
}

void translateM(double tx, double ty, double tz, MATRIX m)
{
	/*
		{1,  0,  0,  0},
		{0,  1,  0,  0},
		{0,  0,  1,  0},
		{tx, ty, tz, 1}};
	*/
	m.m[0] = 1;
	m.m[1] = 0;
	m.m[2] = 0;
	m.m[3] = 0;

	m.m[4] = 0;
	m.m[5] = 1;
	m.m[6] = 0;
	m.m[7] = 0;

	m.m[8] = 0;
	m.m[9] = 0;
	m.m[10] = 1;
	m.m[11] = 0;

	m.m[12] = tx;
	m.m[13] = ty;
	m.m[14] = tz;
	m.m[15] = 1;
}

void rotateX(double theta, double m[][4])
{
	double cosx = cos(theta);
	double sinx = sin(theta);
	/*
			{1,	0, 		0, 		0},
			{0,	cosx, 	sinx, 	0},
			{0,	-sinx, 	cosx, 	0},
			{0,	0, 		0, 		1}};*/
	m[0][0] = 1;
	m[0][1] = 0;
	m[0][2] = 0;
	m[0][3] = 0;

	m[1][0] = 0;
	m[1][1] = cosx;
	m[1][2] = sinx;
	m[1][3] = 0;

	m[2][0] = 0;
	m[2][1] = -sinx;
	m[2][2] = cosx;
	m[2][3] = 0;

	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;
}

void rotateY(double theta, double m[][4])
{
	double cosx = cos(theta);
	double sinx = sin(theta);
	/*
		{cosx, 	0, 	-sinx, 	0},
		{0, 	1, 		0, 	0},
		{sinx, 	0, 	 cosx, 	0},
		{0, 	0, 		0, 	1}};*/
	m[0][0] = cosx;
	m[0][1] = 0;
	m[0][2] = -sinx;
	m[0][3] = 0;

	m[1][0] = 0;
	m[1][1] = 1;
	m[1][2] = 0;
	m[1][3] = 0;

	m[2][0] = sinx;
	m[2][1] = 0;
	m[2][2] = cosx;
	m[2][3] = 0;

	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;
}

void rotateZ(double theta, double m[][4])
{
	double cosx = cos(theta);
	double sinx = sin(theta);
	/*
		{cosx, 	sinx, 	0, 	0},
		{-sinx, cosx, 	0, 	0},
		{0, 	0, 		1, 	0},
		{0, 	0, 		0, 	1}};
	*/
	m[0][0] = cosx;
	m[0][1] = sinx;
	m[0][2] = 0;
	m[0][3] = 0;

	m[1][0] = -sinx;
	m[1][1] = cosx;
	m[1][2] = 0;
	m[1][3] = 0;

	m[2][0] = 0;
	m[2][1] = 0;
	m[2][2] = 1;
	m[2][3] = 0;

	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;
}

void rotate(double theta, POINT3D v, double m[][4])
{
    double d1 = sqrt(v.y * v.y + v.z + v.z);
    double cosAlpha = v.z/d1;
    double sinAlpha = v.y/d1;
    double d2 = sqrt(v.x * v.x + v.z * v.z);
    double cosBeta = v.z/d2;
    double sinBeta = v.x/d2;

    double rAlpha[][4]={
        {1,     0,  0,  0},
        {0, cosAlpha, sinAlpha, 0},
        {0, -sinAlpha, cosAlpha, 0},
        {0,     0,  0,  1}};

    double rBeta[][4]={
        {cosBeta,   0,      sinBeta,    0},
        {0,         1,          0,      0},
        {-sinBeta, 0, cosBeta, 0},
        {0, 0, 0, 1}};

    double rRAlpha[][4] ={
        {1,   0,      0,    0},
        {0,   cosAlpha,     -sinAlpha,      0},
        {0, sinAlpha, cosAlpha, 0},
        {0, 0, 0, 1}};

    double rRBeta[][4]={
        {cosBeta,   0,      -sinBeta,    0},
        {0,         1,          0,      0},
        {sinBeta, 0, cosBeta, 0},
        {0, 0, 0, 1}};

    double AB[4][4];
    double BA[4][4];
    multiply(rAlpha, 4, 4, rBeta, 4, 4, AB);
    multiply(rRBeta, 4, 4, rRAlpha, 4, 4, BA);
    multiply(AB, 4, 4, BA, 4, 4, m);
}

void rotate_1(double theta, double v[], double m[][4])
{

}

void rotate_2(double theta, double vx, double vy, double yz, double m[][4])
{

}

void rotateAboutArbitaryAxis(double theta, POINT3D A, POINT3D B, double m[][4])
{

}

unsigned char scale(double sx, double sy, double sz, double *m[])
{
	/*
			{sx, 0, 0, 1},
			{0, sy, 0, 0},
			{0, 0, sz, 0},
			{0, 0, 0, 1}};
		 * */
	m[0][0] = sx;
	m[0][1] = 0;
	m[0][2] = 0;
	m[0][3] = 1;

	m[1][0] = 0;
	m[1][1] = sy;
	m[1][2] = 0;
	m[1][3] = 0;

	m[2][0] = 0;
	m[2][1] = 0;
	m[2][2] = sz;
	m[2][3] = 0;

	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;

	return 1;
}

unsigned char shear(double xy, double yx, double xz, double zx, double yz, double zy, double *m[])
{
	/*
	 	{1, yx, zx, 1},
		{xy, 1, zy, 0},
		{xz, yz, 1, 0},
		{0, 0, 0, 1}};
	*/
	m[0][0] = 1;
	m[0][1] = yx;
	m[0][2] = zx;
	m[0][3] = 1;

	m[1][0] = xy;
	m[1][1] = 1;
	m[1][2] = zy;
	m[1][3] = 0;

	m[2][0] = xz;
	m[2][1] = yz;
	m[2][2] = 1;
	m[2][3] = 0;

	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;
	return 1;
}

void reflectX(double *m[])
{
	/*
		{1,  0,  0, 0},
		{0, -1,  0, 0},
		{0,  0, -1, 0},
		{0,  0,  0, 1}};
	*/
	m[0][0] = 1;
	m[0][1] = 0;
	m[0][2] = 0;
	m[0][3] = 0;

	m[1][0] = 0;
	m[1][1] = -1;
	m[1][2] = 0;
	m[1][3] = 0;

	m[2][0] = 0;
	m[2][1] = 0;
	m[2][2] = -1;
	m[2][3] = 0;

	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;
}

void reflectXM(MATRIX m)
{
	/*
			{1,  0,  0, 0},
			{0, -1,  0, 0},
			{0,  0, -1, 0},
			{0,  0,  0, 1}};
		*/
	m.m[0] = 1;
	m.m[1] = 0;
	m.m[2] = 0;
	m.m[3] = 0;

	m.m[4] = 0;
	m.m[5] = -1;
	m.m[6] = 0;
	m.m[7] = 0;

	m.m[8] = 0;
	m.m[9] = 0;
	m.m[10] = -1;
	m.m[11] = 0;

	m.m[12] = 0;
	m.m[13] = 0;
	m.m[14] = 0;
	m.m[15] = 1;
}

void reflectY(double *m[])
{
	/*
		{-1,  0,  0, 0},
		{0, 1,  0, 0},
		{0,  0, -1, 0},
		{0,  0,  0, 1}};
	*/
	m[0][0] = -1;
	m[0][1] = 0;
	m[0][2] = 0;
	m[0][3] = 0;

	m[1][0] = 0;
	m[1][1] = 1;
	m[1][2] = 0;
	m[1][3] = 0;

	m[2][0] = 0;
	m[2][1] = 0;
	m[2][2] = -1;
	m[2][3] = 0;

	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;
}

void reflectYM(MATRIX m)
{
	/*
			{-1,  0,  0, 0},
			{0, 1,  0, 0},
			{0,  0, -1, 0},
			{0,  0,  0, 1}};
		*/
	m.m[0] = -1;
	m.m[1] = 0;
	m.m[2] = 0;
	m.m[3] = 0;

	m.m[4] = 0;
	m.m[5] = 1;
	m.m[6] = 0;
	m.m[7] = 0;

	m.m[8] = 0;
	m.m[9] = 0;
	m.m[10] = -1;
	m.m[11] = 0;

	m.m[12] = 0;
	m.m[13] = 0;
	m.m[14] = 0;
	m.m[15] = 1;
}

void reflectZ(double *m[])
{
	/*
			{-1, 0,  0, 0},
			{0, -1,  0, 0},
			{0,  0, 1, 0},
			{0,  0,  0, 1}};
		*/
		m[0][0] = -1;
		m[0][1] = 0;
		m[0][2] = 0;
		m[0][3] = 0;

		m[1][0] = 0;
		m[1][1] = -1;
		m[1][2] = 0;
		m[1][3] = 0;

		m[2][0] = 0;
		m[2][1] = 0;
		m[2][2] = 1;
		m[2][3] = 0;

		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;

}

void reflectZM(MATRIX m)
{
	/*
		{-1, 0,  0, 0},
		{0, -1,  0, 0},
		{0,  0, 1, 0},
		{0,  0,  0, 1}};
			*/
		m.m[0] = -1;
		m.m[1] = 0;
		m.m[2] = 0;
		m.m[3] = 0;

		m.m[4] = 0;
		m.m[5] = -1;
		m.m[6] = 0;
		m.m[7] = 0;

		m.m[8] = 0;
		m.m[9] = 0;
		m.m[10] = 1;
		m.m[11] = 0;

		m.m[12] = 0;
		m.m[13] = 0;
		m.m[14] = 0;
		m.m[15] = 1;
}

